#include  "../include/motor_driver/driver_can.h"


DIRVEDATA  dirveDate_ros;

DriverPub::DriverPub():last_twist_time_(0),line_speed_percent(0),angle_speed_percent(0),queueFlag(0),can_recv_flag(0),rev_left_(0),rev_left_1_(0),rev_right_(0),rev_right_1_(0),last_time_(0){}

DriverPub::~DriverPub()
{
    boost::mutex::scoped_lock look(mutex_);
    l_wheel_mult_ = 0;
    r_wheel_mult_ = 0;
    recv_flag_ = false;
    VCI_CloseDevice(VCI_USBCAN2,0);     //关闭can分析仪
    printf("DirvePub deinit.\n");

}

void DriverPub::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{
    try
    {
        cmd_vel_mutex_.lock();
        last_twist_time_ = ros::Time::now();
        current_twist_ = *msg.get();
        cmd_vel_mutex_.unlock();

    }
    catch(const std::exception& e)
    {
        cmd_vel_mutex_.unlock();
        std::cerr << e.what() << '\n';
    }
}

uint8_t DriverPub::get_line_speed(void)
{
	return (uint8_t) line_speed_percent;
}

uint8_t DriverPub::get_angular_speed(void)
{
	return (uint8_t) angle_speed_percent;
}

//设置线速度和角速度 m/s  rad/s
void DriverPub::set_walkingmotor_speed_angle(double line_speed,double angle_speed)
{
	if(fabs(line_speed) <= MAX_LINEAR)
	{
		line_speed_percent = (int)(line_speed/MAX_LINEAR*100);  //转换成百分数  有符号的整数

	}
	else
	{
		if(line_speed < 0)
		{
			line_speed_percent = -100;
		}
		else
		{
			line_speed_percent = 100;
		}
		
	}

	if(fabs(angle_speed) <= MAX_ANGULAR)
	{
		angle_speed_percent = angle_speed/MAX_ANGULAR*100;       //最大角速度0.7853rad/s
	}
	else
	{
		if(angle_speed < 0)
		{
			angle_speed_percent = -100;
		}
		else
		{
			angle_speed_percent = 100;
		}
		
	}
	
	
}

void DriverPub::setSendData_speed(uint8_t line_speed,uint8_t angular_speed)
{
	int i;
	//需要发送的帧，结构体设置
	VCI_CAN_OBJ send[1];
	send[0].ID = DIRVEE_ID_MOVE_CONTROL;  //0x130
	send[0].SendType=0;
	send[0].RemoteFlag=0;
	send[0].ExternFlag=0;		//标准帧
	send[0].DataLen=8;
	
	send[0].Data[0] = 01;		//can指令
	send[0].Data[1] = 0;		//保留
	send[0].Data[2] = line_speed;		//线速度
	send[0].Data[3] = angular_speed;	//角速度
	send[0].Data[4] = 0;		//保留
	send[0].Data[5] = 0;		//保留
	send[0].Data[6] = 0;		//保留
	send[0].Data[7] = ((send[0].ID>>8)&0xFF)+(send[0].ID&0xFF)+send[0].DataLen+send[0].Data[0]+send[0].Data[1]+send[0].Data[2]+ \
						send[0].Data[3]+send[0].Data[4]+send[0].Data[5]+send[0].Data[6];		//校验和
	if(VCI_Transmit(VCI_USBCAN2, 0, 0, send, 1) == 1)
	{
		//printf("Index:%04d  ",count);count++;
		// printf("CAN1 TX ID:0x%08X",send[0].ID);
		// if(send[0].ExternFlag==0) printf(" Standard ");
		// if(send[0].ExternFlag==1) printf(" Extend   ");
		// if(send[0].RemoteFlag==0) printf(" Data   ");
		// if(send[0].RemoteFlag==1) printf(" Remote ");
		// printf("DLC:0x%02X",send[0].DataLen);
		// printf(" data:0x");

		// for(i=0;i<send[0].DataLen;i++)
		// {
		// 	printf(" %02X",send[0].Data[i]);
		// }

		// printf("\n");
		//send[0].ID+=1;
	}
	else
	{
		printf("can1:send data failed!");
	}

}

void DriverPub::send_speed_callback()
{
    double left_1, right_1;
    double linear_speed, angular_speed;
    short left_des,right_des;

    if((ros::Time::now() - last_twist_time_).toSec() <= 1.0)
    {
        linear_speed = current_twist_.linear.x;
        angular_speed = current_twist_.angular.z;

    } 
    else
    {
        linear_speed = 0;
        angular_speed = 0;
    }
    
    if((ros::Time::now() - now_).toSec() >= 1)
    {
        ROS_WARN_THROTTLE(1,"Didn't received odom data,Please check your connection!");
    }

	if(linear_speed == 0)
	{
		set_walkingmotor_speed_angle(linear_speed,angular_speed);
	}
	else
	{
		if(angular_speed > 0 && angular_speed < start_rotation_limit_w_ && (linear_speed > 0 && linear_speed < 0.1))
		{
			angular_speed = start_rotation_limit_w_;
		}
		if(angular_speed < 0 && angular_speed > -start_rotation_limit_w_ && (linear_speed > -0.1 && linear_speed < 0))
		{
			angular_speed = -start_rotation_limit_w_;
		}
		 set_walkingmotor_speed_angle(linear_speed,angular_speed);
	}
   
    setSendData_speed(get_line_speed(),get_angular_speed());
}

bool DriverPub::check_sum(VCI_CAN_OBJ * rec_buff)
{
	static int count = 0;
	uint8_t sum=0;
	int ind=0;
	// printf("Index:%04d  ",count);count++;//序号递增
	// printf("CAN%d RX ID:0x%08X", ind+1, rec_buff->ID);//ID
	// if(rec_buff->ExternFlag==0) printf(" Standard ");//帧格式：标准帧
	// if(rec_buff->ExternFlag==1) printf(" Extend   ");//帧格式：扩展帧
	// if(rec_buff->RemoteFlag==0) printf(" Data   ");//帧类型：数据帧
	// if(rec_buff->RemoteFlag==1) printf(" Remote ");//帧类型：远程帧
	// printf("DLC:0x%02X",rec_buff->DataLen);//帧长度
	// printf(" data:0x");	//数据
	// for(int i = 0; i < rec_buff->DataLen; i++)
	// {
	// 	printf(" %02X", rec_buff->Data[i]);
	// }
	// printf(" TimeStamp:0x%08X",rec_buff->TimeStamp);//时间标识。
	// printf("\n");
	//校验和
	sum = (((rec_buff->ID>>8)&0xFF)+(rec_buff->ID&0xFF)+(rec_buff->DataLen&0xFF)+(rec_buff->Data[0]&0xFF)+rec_buff->Data[1]+rec_buff->Data[2]+ \
		rec_buff->Data[3]+rec_buff->Data[4]+rec_buff->Data[5]+rec_buff->Data[6]);
	// printf("sum = %d\r\n",sum);
	// printf("rec_buff->Data[7] = %d\r\n",rec_buff->Data[7]);
	if(sum ==rec_buff->Data[7])
	{
		return true;		
	}
	else
	{
		return false;
	}
	

}

//采集底盘编码器值
void DriverPub::Calculate_LeftAndRight_Meter(VCI_CAN_OBJ *p)
{
	int temp_buff = 0;
	temp_buff = ((p->Data[0]<<24)|p->Data[1]<<16|p->Data[2]<<8|p->Data[3]);     //编码器
	// temp_buff = ((temp_buff<0)? (-temp_buff):temp_buff);

	if(p->ID == DIRVEE_ID_ONE_POSE_ACK)
	{
		rev_left_ = temp_buff;
		queueFlag |= 1<<0;
		// printf("rev_left_:%d\n",rev_left_);
	}
	if(p->ID == DIRVEE_ID_TWO_POSE_ACK)
	{
		rev_right_ = temp_buff;
		queueFlag |= 1<<1;
		// printf("rev_right_:%d\n",rev_right_);
	}
	if(p->ID == DIRVEE_ID_THREE_POSE_ACK)
	{
		rev_right_1_ = temp_buff;		
		queueFlag |= 1<<2;
		// printf("rev_right_1_:%d\n",rev_right_1_);
	}
	if(p->ID == DIRVEE_ID_FOUR_POSE_ACK)
	{
		rev_left_1_ = temp_buff;
		queueFlag |= 1<<3;
		// printf("rev_left_1_:%d\n",rev_left_1_);
	}

	if(queueFlag == 0xF)		//接收四轮编码器数值
	{
		queueFlag = 0;
		can_recv_flag = 1;			//接收标志
	}



}

//处理上报来的底盘数据  Process the data submitted by the chassis
void DriverPub::dirve_ProcessData(VCI_CAN_OBJ *p,DIRVEDATA *PdirveDate )
{
	
	switch(p->ID)
	{
		case DIRVEE_ID_SYSTEM_STATE_ACK:
			PdirveDate->carBodyStateAck = p->Data[0];
			PdirveDate->modeControlAck = p->Data[1];
			PdirveDate->batteryVoltageAck = p->Data[2]<<8|p->Data[3];
			PdirveDate->faultMessageAck = p->Data[4]<<8|p->Data[5];
		break;
		case DIRVEE_ID_MOVE_CONTROL_ACK:
			PdirveDate->movementSpeedAck = p->Data[0]<<8|p->Data[1];
   			PdirveDate->rotationalSpeedAck = p->Data[2]<<8|p->Data[3];
		break;
		case DIRVEE_ID_MOVE_CONTROL:
			PdirveDate->controlMode = p->Data[0];
   			PdirveDate->faultClearance = p->Data[1];
			PdirveDate->percentLinear = p->Data[2];
			PdirveDate->percentAngular =  p->Data[3];
		break;
		case DIRVEE_ID_ONE_MOTOR_ACK:
			PdirveDate->oneMotorElectricCurrentAck = p->Data[0]<<8|p->Data[1];
   			PdirveDate->oneMotorRorateSpeedAck = p->Data[2]<<8|p->Data[3];
   			PdirveDate->oneMotorTemperatureAck = p->Data[4];
		break;
		case DIRVEE_ID_TWO_MOTOR_ACK:
			PdirveDate->twoMotorElectricCurrentAck = p->Data[0]<<8|p->Data[1];
   			PdirveDate->twoMotorRorateSpeedAck = p->Data[2]<<8|p->Data[3];
   			PdirveDate->twoMotorTemperatureAck = p->Data[4];
		break;
		case DIRVEE_ID_THREE_MOTOR_ACK:
			PdirveDate->threeMotorElectricCurrentAck = p->Data[0]<<8|p->Data[1];
   			PdirveDate->threeMotorRorateSpeedAck = p->Data[2]<<8|p->Data[3];
   			PdirveDate->threeMotorTemperatureAck = p->Data[4];
		break;
		case DIRVEE_ID_FOUR_MOTOR_ACK:
			PdirveDate->fourMotorElectricCurrentAck = p->Data[0]<<8|p->Data[1];
   			PdirveDate->fourMotorRorateSpeedAck = p->Data[2]<<8|p->Data[3];
   			PdirveDate->fourMotorTemperatureAck = p->Data[4];
		break;
		case DIRVEE_ID_LAMPLIGHT_CONTROL:
			PdirveDate->lightControlEnableSign = p->Data[0];
   			PdirveDate->bodyLightPattern = p->Data[1];
		break;
		case DIRVEE_ID_LAMPLIGHT_CONTROL_ACK:
			PdirveDate->currentLightControlEnableSignAck = p->Data[0];
  			PdirveDate->bodyLightPatternAck = p->Data[1];
		break;
		case DIRVEE_ID_ONE_POSE_ACK:
		case DIRVEE_ID_TWO_POSE_ACK:
		case DIRVEE_ID_THREE_POSE_ACK:
		case DIRVEE_ID_FOUR_POSE_ACK:
		     Calculate_LeftAndRight_Meter(p);
			 break;
		default:
		break;


	}

}

uint8_t DriverPub::get_recvFlag(void)
{
	return can_recv_flag;

}

int32_t DriverPub::get_rev_leftVol(void)
{
	return rev_left_;
}


int32_t DriverPub::get_rev_rightVol(void)
{
	return rev_right_;
}

void DriverPub::handle_speed_data(void)
{
    now_ = ros::Time::now();
    delta_time_ = (now_ - last_time_).toSec();
    // rev_left_ = get_rev_leftVol();
    // rev_right_ = get_rev_rightVol();
    if(start_flag_)
    {
        accumulation_y_ = 0.0;
        accumulation_x_ = 0.0;
        accumulation_th_ = 0.0;

		accumulation_left_ = 0.0;
    	accumulation_right_ = 0.0;
		accumulation_right_1_ = 0.0;
		accumulation_left_1_ = 0.0;


        cur_left_ = rev_left_;
		cur_left_1_ = rev_left_1_;
        cur_right_= rev_right_;
		cur_right_1_ = rev_right_1_;

		delta_left_ = 0;
        delta_right_ = 0;
		delta_left_1_= 0;
        delta_right_1_= 0;

        last_time_ = now_;
        start_flag_ = false;
        ROS_INFO("ROBOT is Successfuly connected!");
        // ROS_INFO_STREAM("The Code Version is" << code_version_);

    }

    if(cur_left_ == 0 && cur_right_ == 0)
    {
        delta_left_ = 0;
        delta_right_ = 0;
		delta_left_1_= 0;
        delta_right_1_= 0;
    }
    else
    {
        if(rev_left_< encoder_low_wrap_ && cur_left_ > encoder_high_wrap_)
        {
            l_wheel_mult_++;

        }
        else if(rev_left_ > encoder_high_wrap_ && cur_left_ < encoder_low_wrap_)
        {
            l_wheel_mult_--;
        }
        else
        {
            l_wheel_mult_ = 0;
        }

		 if(rev_left_1_< encoder_low_wrap_ && cur_left_1_ > encoder_high_wrap_)
        {
            l_wheel_mult_1_++;

        }
        else if(rev_left_1_ > encoder_high_wrap_ && cur_left_1_< encoder_low_wrap_)
        {
            l_wheel_mult_1_--;
        }
        else
        {
            l_wheel_mult_1_ = 0;
        }


        if(rev_right_ < encoder_low_wrap_ && cur_right_ > encoder_high_wrap_)
        {
            r_wheel_mult_++;
        }  
        else if(rev_right_ > encoder_high_wrap_ && cur_right_ < encoder_low_wrap_)
        {
            r_wheel_mult_--;
        }    
        else
        {
            r_wheel_mult_ = 0;
        }

		 if(rev_right_1_ < encoder_low_wrap_ && cur_right_1_ > encoder_high_wrap_)
        {
            r_wheel_mult_1_++;
        }  
        else if(rev_right_1_ > encoder_high_wrap_ && cur_right_1_ < encoder_low_wrap_)
        {
            r_wheel_mult_1_--;
        }    
        else
        {
            r_wheel_mult_1_ = 0;
        }


        delta_left_ = 1.0*(rev_left_ + (l_wheel_mult_ * (encoder_max_ - encoder_min_)-cur_left_))/ticks_per_meter_;
        delta_right_ = 1.0*(rev_right_ + (r_wheel_mult_ * (encoder_max_ - encoder_min_)-cur_right_))/ticks_per_meter_;
		
        delta_right_1_ = 1.0*(rev_right_1_ + (r_wheel_mult_1_ * (encoder_max_ - encoder_min_)-cur_right_1_))/ticks_per_meter_;
       delta_left_1_ = 1.0*(rev_left_1_ + (l_wheel_mult_1_ * (encoder_max_ - encoder_min_)-cur_left_1_))/ticks_per_meter_;
    }

	accumulation_left_ += (delta_left_/(wheel_diameter_4wd_/2.0));
    accumulation_right_ += (delta_right_/(wheel_diameter_4wd_/2.0));
	accumulation_right_1_ += (delta_right_1_/(wheel_diameter_4wd_/2.0));
	accumulation_left_1_ += (delta_left_1_/(wheel_diameter_4wd_/2.0));

	left_right_wheel_state_.front_left_joint.position  = accumulation_left_;
	left_right_wheel_state_.front_right_joint.position = accumulation_right_;
	left_right_wheel_state_.rear_right_joint.position = accumulation_right_1_;
	left_right_wheel_state_.rear_left_joint.position = accumulation_left_1_;

	wheel_state_feedback_.publish(left_right_wheel_state_);


    cur_left_ = rev_left_;
    cur_right_ = rev_right_;
	cur_left_1_ = rev_left_1_;
    cur_right_1_ = rev_right_1_;

	 delta_xy_ave_ = ((delta_left_+delta_left_1_)/2.0+(delta_right_+delta_right_1_)/2.0)/2.0;
	 delta_th_ =  ((delta_right_+delta_right_1_)/2.0 - (delta_left_+delta_left_1_)/2.0)/wheel_track_4wd_;
    // delta_xy_ave_ = (delta_left_ + delta_right_)/2.0;           //整车的位移量
    // delta_th_ = (delta_right_ - delta_left_)/wheel_track_4wd_;  //整车的角位移量

    v_linear_ = delta_xy_ave_ /delta_time_;   //速度
    v_angular_ = delta_th_ / delta_time_;    //角速度
 	// printf("v_linear_:%lf    \n",v_linear_);
  	// printf("v_angular_:%lf    \n",v_angular_);
	// printf("delta_xy_ave_:%lf    \n",delta_xy_ave_);
	// printf("delta_th_:%lf    \n",delta_th_);

    if(delta_xy_ave_ !=0)
    {
        delta_x_ = cos(delta_th_) * delta_xy_ave_;    //X方向上的位移量
        delta_y_ = -sin(delta_th_)* delta_xy_ave_;    //Y方向上的位移量

        //累积位移量
        accumulation_x_ += (cos(accumulation_th_)*delta_x_ - sin(accumulation_th_)*delta_y_);
        accumulation_y_ += (sin(accumulation_th_)*delta_x_ + cos(accumulation_th_)*delta_y_);
		// printf("accumulation_x_:%lf   accumulation_y_:%lf  \n",accumulation_x_,accumulation_y_);
		 

    }

    if(delta_th_ != 0)
    {
        accumulation_th_ += delta_th_;
    }
    // printf("accumulation_th_:%lf    \n",accumulation_th_);
// 	v_linear_ = 0;
// 	v_angular_ = 0;
// 	accumulation_x_ = 0;
// 	accumulation_y_ = 0;
//    accumulation_th_ = 0;
    // transformStamped_.header.stamp    = ros::Time::now();
	transformStamped_.header.stamp    =now_;
    transformStamped_.header.frame_id = odom_frame_;
    transformStamped_.child_frame_id  = base_frame_;
    transformStamped_.transform.translation.x = accumulation_x_;
    transformStamped_.transform.translation.y = accumulation_y_;
    transformStamped_.transform.translation.z = 0.0;
    tf2::Quaternion q;
    q.setRPY(0,0,accumulation_th_);         //欧拉角转四元素
    transformStamped_.transform.rotation.x = q.x();
    transformStamped_.transform.rotation.y = q.y();
    transformStamped_.transform.rotation.z = q.z();
    transformStamped_.transform.rotation.w = q.w();

	//   transformStamped_.transform.rotation.x = 0;
    // transformStamped_.transform.rotation.y = 0;
    // transformStamped_.transform.rotation.z = 0;
    // transformStamped_.transform.rotation.w = 1;

    if(publish_odom_transform_)                     //初始化 ture
        br_.sendTransform(transformStamped_);       //发布车体角度TF坐标
    
    odom_.header.frame_id = odom_frame_;
    odom_.child_frame_id  = base_frame_;
    odom_.header.stamp    = now_;
    odom_.pose.pose.position.x = accumulation_x_;
    odom_.pose.pose.position.y = accumulation_y_;
    odom_.pose.pose.position.z = 0;
    odom_.pose.pose.orientation.x = q.getX();
    odom_.pose.pose.orientation.y = q.getY();
    odom_.pose.pose.orientation.z = q.getZ();
    odom_.pose.pose.orientation.w = q.getW();

	// odom_.pose.pose.orientation.x = 0;
    // odom_.pose.pose.orientation.y = 0;
    // odom_.pose.pose.orientation.z = 0;
    // odom_.pose.pose.orientation.w = 0;

    odom_.twist.twist.linear.x = v_linear_;
    odom_.twist.twist.linear.y = 0;
    odom_.twist.twist.angular.z = v_angular_;
    odom_.twist.covariance = { 1e-9, 0, 0, 0, 0, 0, 
                              0, 1e-3, 1e-9, 0, 0, 0, 
                              0, 0, 1e6, 0, 0, 0,
                              0, 0, 0, 1e6, 0, 0, 
                              0, 0, 0, 0, 1e6, 0, 
                              0, 0, 0, 0, 0, 1e-9 };
    odom_.pose.covariance = { 1e-9, 0, 0, 0, 0, 0, 
                              0, 1e-3, 1e-9, 0, 0, 0, 
                              0, 0, 1e6, 0, 0, 0,
                              0, 0, 0, 1e6, 0, 0, 
                              0, 0, 0, 0, 1e6, 0, 
                              0, 0, 0, 0, 0, 1e-9 };

    odom_pub_.publish(odom_);
    
    last_time_ = now_;

}

void DriverPub::receive_func()  //接收线程。
{
	int reclen=0;
	
	VCI_CAN_OBJ rec[3000];//接收缓存，设为3000为佳。
	int i,j;
    int ind=0;
	
	while(true)
	{
		if((reclen=VCI_Receive(VCI_USBCAN2,0,ind,rec,3000,100))>0)//调用接收函数，如果有数据，进行数据处理显示。
		{
			for(j=0;j<reclen;j++)
			{
				
				//判断校验
				if(check_sum(&rec[j]))
				{
					dirve_ProcessData(&rec[j],&dirveDate_ros);

				}
				else
				{
					printf("校验错误\r\n");
				}
				
			}
		}
		else
		{
			// printf("接收超时\r\n");
		}

		if(get_recvFlag())
		{
			handle_speed_data();
			can_recv_flag = 0;
		}
	}
	printf("run thread exit\n");//退出接收线程		

}
bool DriverPub::initRobot()
{
    int num=0;
	num=VCI_FindUsbDevice2(pInfo1);		//获取设备信息，返回设备数量
	printf(">>can communication init !\r\n");//指示程序已运行
	printf(">>USBCAN DEVICE NUM:");printf("%d", num);printf(" PCS");printf("\n");

		for(int i=0;i<num;i++)
		{
		printf("Device:");printf("%d", i);printf("\n");
                printf(">>Get VCI_ReadBoardInfo success!\n");
		
		printf(">>Serial_Num:%c", pInfo1[i].str_Serial_Num[0]);
		printf("%c", pInfo1[i].str_Serial_Num[1]);
		printf("%c", pInfo1[i].str_Serial_Num[2]);
		printf("%c", pInfo1[i].str_Serial_Num[3]);
		printf("%c", pInfo1[i].str_Serial_Num[4]);
		printf("%c", pInfo1[i].str_Serial_Num[5]);
		printf("%c", pInfo1[i].str_Serial_Num[6]);
		printf("%c", pInfo1[i].str_Serial_Num[7]);
		printf("%c", pInfo1[i].str_Serial_Num[8]);
		printf("%c", pInfo1[i].str_Serial_Num[9]);
		printf("%c", pInfo1[i].str_Serial_Num[10]);
		printf("%c", pInfo1[i].str_Serial_Num[11]);
		printf("%c", pInfo1[i].str_Serial_Num[12]);
		printf("%c", pInfo1[i].str_Serial_Num[13]);
		printf("%c", pInfo1[i].str_Serial_Num[14]);
	 	printf("%c", pInfo1[i].str_Serial_Num[15]);
		printf("%c", pInfo1[i].str_Serial_Num[16]);
		printf("%c", pInfo1[i].str_Serial_Num[17]);
		printf("%c", pInfo1[i].str_Serial_Num[18]);
		printf("%c", pInfo1[i].str_Serial_Num[19]);printf("\n");

		printf(">>hw_Type:%c", pInfo1[i].str_hw_Type[0]);
		printf("%c", pInfo1[i].str_hw_Type[1]);
		printf("%c", pInfo1[i].str_hw_Type[2]);
		printf("%c", pInfo1[i].str_hw_Type[3]);
		printf("%c", pInfo1[i].str_hw_Type[4]);
		printf("%c", pInfo1[i].str_hw_Type[5]);
		printf("%c", pInfo1[i].str_hw_Type[6]);
		printf("%c", pInfo1[i].str_hw_Type[7]);
		printf("%c", pInfo1[i].str_hw_Type[8]);
		printf("%c", pInfo1[i].str_hw_Type[9]);printf("\n");	

		printf(">>Firmware Version:V");
		printf("%x", (pInfo1[i].fw_Version&0xF00)>>8);
		printf(".");
		printf("%x", (pInfo1[i].fw_Version&0xF0)>>4);
		printf("%x", pInfo1[i].fw_Version&0xF);
		printf("\n");
	}
	printf(">>\n");
	printf(">>\n");
	printf(">>\n");
/*
>>can communication init !
>>USBCAN DEVICE NUM:1 PCS
Device:0
>>Get VCI_ReadBoardInfo success!
>>Serial_Num:01701020B0F
>>hw_Type:CAN-Linux
>>Firmware Version:V3.24
>>
>>
>>
*/


	if(VCI_OpenDevice(VCI_USBCAN2,0,0)==1)//打开设备
	{
		printf(">>open deivce success!\n");//打开设备成功
	}else
	{
		printf(">>open deivce error!\n");
        return 0;
		exit(1);
	}
	if(VCI_ReadBoardInfo(VCI_USBCAN2,0,&pInfo)==1)//读取设备序列号、版本等信息。
	{
                printf(">>Get VCI_ReadBoardInfo success!\n");
		
		//printf(" %08X", pInfo.hw_Version);printf("\n");
		//printf(" %08X", pInfo.fw_Version);printf("\n");
		//printf(" %08X", pInfo.dr_Version);printf("\n");
		//printf(" %08X", pInfo.in_Version);printf("\n");
		//printf(" %08X", pInfo.irq_Num);printf("\n");
		//printf(" %08X", pInfo.can_Num);printf("\n");
		printf(">>Serial_Num:%c", pInfo.str_Serial_Num[0]);
		printf("%c", pInfo.str_Serial_Num[1]);
		printf("%c", pInfo.str_Serial_Num[2]);
		printf("%c", pInfo.str_Serial_Num[3]);
		printf("%c", pInfo.str_Serial_Num[4]);
		printf("%c", pInfo.str_Serial_Num[5]);
		printf("%c", pInfo.str_Serial_Num[6]);
		printf("%c", pInfo.str_Serial_Num[7]);
		printf("%c", pInfo.str_Serial_Num[8]);
		printf("%c", pInfo.str_Serial_Num[9]);
		printf("%c", pInfo.str_Serial_Num[10]);
		printf("%c", pInfo.str_Serial_Num[11]);
		printf("%c", pInfo.str_Serial_Num[12]);
		printf("%c", pInfo.str_Serial_Num[13]);
		printf("%c", pInfo.str_Serial_Num[14]);
		printf("%c", pInfo.str_Serial_Num[15]);
		printf("%c", pInfo.str_Serial_Num[16]);
		printf("%c", pInfo.str_Serial_Num[17]);
		printf("%c", pInfo.str_Serial_Num[18]);
		printf("%c", pInfo.str_Serial_Num[19]);printf("\n");

		printf(">>hw_Type:%c", pInfo.str_hw_Type[0]);
		printf("%c", pInfo.str_hw_Type[1]);
		printf("%c", pInfo.str_hw_Type[2]);
		printf("%c", pInfo.str_hw_Type[3]);
		printf("%c", pInfo.str_hw_Type[4]);
		printf("%c", pInfo.str_hw_Type[5]);
		printf("%c", pInfo.str_hw_Type[6]);
		printf("%c", pInfo.str_hw_Type[7]);
		printf("%c", pInfo.str_hw_Type[8]);
		printf("%c", pInfo.str_hw_Type[9]);printf("\n");

		printf(">>Firmware Version:V");
		printf("%x", (pInfo.fw_Version&0xF00)>>8);
		printf(".");
		printf("%x", (pInfo.fw_Version&0xF0)>>4);
		printf("%x", pInfo.fw_Version&0xF);
		printf("\n");	
	}else
	{
		printf(">>Get VCI_ReadBoardInfo error!\n");
        return 0;
		exit(1);
	}
	
	//初始化参数，严格参数二次开发函数库说明书。
	VCI_INIT_CONFIG config;
	config.AccCode=0;
	config.AccMask=0xFFFFFFFF;
	config.Filter=1;//接收所有帧
	config.Timing0=0x00;/*波特率500 Kbps  0x00  0x1C*/

	config.Timing1=0x1C;
	config.Mode=0;//正常模式		
	
	if(VCI_InitCAN(VCI_USBCAN2,0,0,&config)!=1)
	{
		printf(">>Init CAN1 error\n");
		VCI_CloseDevice(VCI_USBCAN2,0);
	}

	if(VCI_StartCAN(VCI_USBCAN2,0,0)!=1)
	{
		printf(">>Start CAN1 error\n");
		VCI_CloseDevice(VCI_USBCAN2,0);

	}

	if(VCI_InitCAN(VCI_USBCAN2,0,1,&config)!=1)
	{
		printf(">>Init can2 error\n");
		VCI_CloseDevice(VCI_USBCAN2,0);

	}
	if(VCI_StartCAN(VCI_USBCAN2,0,1)!=1)
	{
		printf(">>Start can2 error\n");
		VCI_CloseDevice(VCI_USBCAN2,0);

	}

    return 1;
}

void DriverPub::loop()
{

    ros::NodeHandle n("~");
    ros::Rate loop_rate(10);

    //获取配置参数
	n.param<std::string>("odom_frame",odom_frame_,std::string("odom"));
    n.param<std::string>("base_frame",base_frame_,std::string("base_footprint"));
    n.param<double>("encoder_resolution_4wd",encoder_resolution_4wd_,10000);
    n.param<double>("wheel_track_4wd",wheel_track_4wd_,0.56);
    n.param<double>("linear_correction_factor",linear_correction_factor_,1.0);
    n.param<double>("angular_correction_factor",angular_correction_factor_,1.0);
    n.param<double>("gear_reduction_4wd",gear_reduction_4wd_,35);
    n.param<double>("wheel_diameter_4wd",wheel_diameter_4wd_,0.35);
    n.param<double>("start_rotation_limit_w",start_rotation_limit_w_,0.4);								//角速度限速
    n.param<bool>("start_flag",start_flag_,true);
    n.param<int>("encoder_min",encoder_min_,-2147483648);
    n.param<int>("encoder_max",encoder_max_,2147483647);
    // n.param<double>("encoder_low_wrap",(encoder_max_-encoder_min_)*0.3 + encoder_min_);
    // n.param<double>("encoder_high_wrap",(encoder_max_-encoder_min_)*0.7 + encoder_min_);
    n.param<int>("l_wheel_mult",l_wheel_mult_,0);
    n.param<int>("r_wheel_mult",r_wheel_mult_,0);
    n.param<bool>("publish_odom_transform",publish_odom_transform_,false);
 
	encoder_low_wrap_ = (encoder_max_-encoder_min_)*0.3 + encoder_min_;
	encoder_high_wrap_ = (encoder_max_-encoder_min_)*0.7 + encoder_min_;

    int count = 0;
    //计算里程计关键参数
    encoder_resolution_4wd_ = encoder_resolution_4wd_/linear_correction_factor_;
    wheel_track_4wd_ = wheel_track_4wd_/angular_correction_factor_;

    ticks_per_meter_ = encoder_resolution_4wd_ * gear_reduction_4wd_/(wheel_diameter_4wd_ * M_PI); //(10000 * 35)/(0.35*PI)==脉冲数/m
	if(initRobot())
    {
        odom_pub_ = n.advertise<nav_msgs::Odometry>("/odom", 10);
        cmd_sub_ = n.subscribe<geometry_msgs::Twist>("/cmd_vel",100,&DriverPub::cmd_vel_callback,this);

		wheel_state_feedback_=n.advertise<motor_driver::Feedback>("/motor_driverFeedback",10);


        //初始化USB—CAN线程，接受数据
        boost::thread recv_thread(boost::bind(&DriverPub::receive_func,this));
        // ret=pthread_create(&threadid,NULL,receive_func,&m_run0);
        now_ = ros::Time::now();

        while(ros::ok())
        {

                send_speed_callback();
                // usleep(10000);
                // receive_func();
                

            ros::spinOnce();
            loop_rate.sleep();
            

        }

    }


}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "DirvePub_node");
    DriverPub driverCan;
    driverCan.loop();
 
    return 0;
}
